
import RPi.GPIO as gpio
import RPi.GPIO as GPIO
import time
import thread
from threading import *

GPIO_SIG = 8
#Motor 1 GPIO Pin
IC1A = 25
IC2A = 10
#Motor 2 GPIO Pin
IC3A = 17
IC4A = 4
#get semaphore
sem=Semaphore()

def getAndPrint():
	for i in range(100):
            global sem
            sem.acquire()
            measurementInCM()
            sem.release()
	gpio.cleanup()

def measurementInCM():
	gpio.setmode(gpio.BOARD)
	gpio.setup(GPIO_SIG, gpio.OUT)
	gpio.output(GPIO_SIG, gpio.LOW)
	time.sleep(0.2)
	gpio.output(GPIO_SIG, gpio.HIGH)
	time.sleep(0.5)
	gpio.output(GPIO_SIG, gpio.LOW)
	start = time.time()

	gpio.setup(GPIO_SIG, gpio.IN)

	while gpio.input(GPIO_SIG) == 0:
		start = time.time()
		
	while gpio.input(GPIO_SIG) == 1:
		stop = time.time()
		
	measurementPulse(start, stop)

def measurementPulse(start, stop):

	elapsed = stop-start

	distance = elapsed * 34300

	distance = distance / 2

	if distance <= 20:
            gpio.setmode(gpio.BCM)
            gpio.output(IC1A, gpio.LOW)
            gpio.output(IC2A, gpio.LOW)
            gpio.output(IC3A, gpio.LOW)
            gpio.output(IC4A, gpio.LOW)
            
        print "Distance : %.lf CM" % distance

def forward():
	global sem
	sem.acquire()
	gpio.setmode(gpio.BCM)
   	gpio.output(IC3A, gpio.HIGH)
   	gpio.output(IC4A, gpio.HIGH)
	sem.release()
def backward():
	global sem
	sem.acquire()
   	gpio.setmode(gpio.BCM)
   	gpio.output(IC3A, gpio.HIGH)
   	gpio.output(IC4A, gpio.LOW)
	sem.release()
def left():
	global sem
	sem.acquire()
   	gpio.setmode(gpio.BCM)
   	gpio.output(IC1A, gpio.HIGH)
   	gpio.output(IC2A, gpio.HIGH)
	sem.release()
def right():
	global sem
	sem.acquire()
   	gpio.setmode(gpio.BCM)
   	gpio.output(IC1A, gpio.LOW)
   	gpio.output(IC2A, gpio.HIGH)
	sem.release()
def stop():
	global sem
	sem.acquire()
   	gpio.setmode(gpio.BCM)
   	gpio.output(IC1A, gpio.LOW)
   	gpio.output(IC2A, gpio.LOW)
   	gpio.output(IC3A, gpio.LOW)
   	gpio.output(IC4A, gpio.LOW)
	sem.release()

gpio.setwarnings(False)
gpio.setmode( gpio.BCM )

#Pin Output Setup
gpio.setup(IC1A, gpio.OUT)
gpio.setup(IC2A, gpio.OUT)
gpio.setup(IC3A, gpio.OUT)
gpio.setup(IC4A, gpio.OUT)

#Pin Initialization
gpio.output(IC1A, gpio.LOW)
gpio.output(IC2A, gpio.LOW)
gpio.output(IC3A, gpio.LOW)
gpio.output(IC4A, gpio.LOW)

#thread start
thread.start_new_thread(getAndPrint, ())
    
	
